/*
 * Vector2d.java
 *
 * Created on 2 de agosto de 2007, 06:00 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Utils;

/**
 *
 * @author jonathan
 */
public class Vector2d {
    
    public static final int MODE_XY=0;
    public static final int MODE_MA=1;
    double x;
    double y;
    double angle;
    double magnitud;
    
    /**
     * Creates a new instance of Vector2d
     */
    public Vector2d()
    {
        x = 0;
        y = 0;
        magnitud = 0;
        angle = 0;
                
    
    }
    public Vector2d(Vector2d vec)
    {
        this.x = vec.getX();
        this.y = vec.getY();
        this.magnitud = vec.getMag();
        this.angle = vec.getAngle();

    }
    public Vector2d(double val1, double val2, int mode) {
        if(mode==MODE_XY){
            x=val1;
            y=val2;
            
                angle=(float)Math.atan2((float)y,(float)x);
                magnitud=(float)Math.sqrt((double)(x*x+y*y));
            
        }
        else{
            magnitud=val1;
            angle=val2;
            
                x=magnitud*(float)Math.cos((double) angle);
                y=magnitud*(float)Math.sin((double) angle);
            
        }
    
    }
    
    public void print(int mode)
    {
        if(mode == this.MODE_XY)
            System.out.println("VECTOR2D: (x, y): ("+this.getX()+", "+this.getY()+")\n");
        else
            System.out.println("VECTOR2D: (mag, theta): ("+this.getMag()+", "+ Math.toDegrees(this.getAngle())+")");
        
    }
    
    public void setX(float x){
        this.x=x;
        angle=(float)Math.atan2((float)y,(float)x);
        magnitud=(float)Math.sqrt((double)(x*x+y*y));
    }
    
    public void setY(float y){
        this.y=y;
        angle=(float)Math.atan2((float)y,(float)x);
        magnitud=(float)Math.sqrt((double)(x*x+y*y));
    }
    
    public void setAngle(float theta){
        this.angle=theta;
        if(magnitud!=0){
            x=magnitud*(float)Math.cos((double) theta);
            y=magnitud*(float)Math.sin((double) theta);
        }
        else
        {
            x=0;
            y=0;
        }
    }
    //FIXME: Revisar cuuando se pasa una magnitud negativa!!!
    public void setMag(float mag){
        magnitud=mag;
        if(magnitud!=0){
            x=magnitud*(float)Math.cos((double) angle);
            y=magnitud*(float)Math.sin((double) angle);
        }
        else
        {
            x=0;
            y=0;
        }
    }
    
    public double getX(){ return x;}
    public double getY(){ return y;}
    public double getAngle(){ return angle;}
    public double getMag(){ return magnitud;}
    
    public Vector2d add(Vector2d vector){
        return new Vector2d(this.x+vector.getX(), this.y+vector.getY(),MODE_XY);
    }
    
    /**
     * Sub
     * @param vector escala a aplicar
     * @return Vector new vector after scale
     */
    public Vector2d sub(Vector2d vector){
        return new Vector2d(this.x-vector.getX(), this.y-vector.getY(), MODE_XY);
    }
    
    /**
     * This method .. scale the vector
     * @param escala escala a aplicar
     * @return Vector new vector after scale
     */
    public Vector2d scale(double escala){
        return new Vector2d(escala*this.x,escala*this.y,MODE_XY);
    }
    
    /**
     * Normaliza el vector.
     * @return Vector new vector after normalize
     */
    public Vector2d normalize(){
        if(magnitud!=0)
            return new Vector2d(this.x/this.magnitud,this.y/this.magnitud,MODE_XY);
        else
            return new Vector2d(0,0,MODE_XY);
    }
    
    /**
     * Rota el plano de referencia del robot a angle en radianes.
     * @param angle angulo al que se rotara en radianes
     * @return Vector new vector after rotation
     */
    public Vector2d rotate(double angle){
        return new Vector2d(this.magnitud,(this.angle+angle),MODE_MA);
    }

    //correct angle in order to be in the range [0, 2*pi]
    public static double angleCorrZero22PI(double angle)
    {
        double newAngle = angleCorrMPI2PI(angle);

        if(newAngle<0)
            newAngle += 2*Math.PI;

        return newAngle;


    }
    //correct angle in order to be in the range [-pi, pi]
    public static double angleCorrMPI2PI(double angle)
    {
        int laps = 0;
        
        //correct laps if angle is higher than one lap (2 PI)
        if(Math.abs(angle)>2*Math.PI)
        {
            //eliminate laps from angle
            laps = (int)(angle / (2*Math.PI));
            angle -= laps*2*Math.PI;
            
        }
        //System.out.println("Angle after laps: "+angle);
        //then correct angle in order to have a result between [-PI, PI]
        if(Math.abs(angle)>Math.PI)
        {
            if(angle<0)
                angle += 2*Math.PI;
            else
                angle -= 2*Math.PI;
        }
        return angle;
    
    }

    public static boolean angleGreaterThan(double angle1, double angle2)
    {
        double a1 = angleCorrMPI2PI(angle1);
        double a2 = angleCorrMPI2PI(angle2);

        if(a1<0)
            a1 += 2*Math.PI;
        if(a2<0)
            a2 += 2*Math.PI;

        System.out.println("a1: "+a1+" a2: "+a2);
        return a2>a1;

    }
    public double distanceTo(Vector2d point){
        return Math.sqrt(Math.pow((this.x-point.getX()),2.0f)+Math.pow((this.y-point.getY()),2.0f));
    }
    
    static public Vector2d averageVector(Vector2d [] vectors){
        float sumX=0;
        float sumY=0;
        for(int i=0;i<vectors.length;i++){
            sumX+=vectors[i].getX();
            sumY+=vectors[i].getY();
        }
        sumX/=vectors.length;
        sumY/=vectors.length;
        
        return new Vector2d(sumX,sumY,MODE_XY);
    }
    
    public String toString(){
        return new String("X= "+this.x+"\tY= "+this.y+"\n");
    }
}
